#include "StdAfx.h"
#include "ChmDevice.h"
#include <valarray> // for sqrt function

using namespace HapticDevice;

#pragma unmanaged
ChmDevice::ChmDevice(wstring deviceName)
{
	/// <summary>
	/// hardware settings
	/// </summary>
	this->deviceName	= deviceName;
	ipAddress			= L"10.30.203.48";
	port				= 4321;
	hm					= NULL;

	/// <summary>
	/// haptic environment settings
	/// </summary>
	currentState		= FCSSTATE_UNKNOWN;
	mass				= 5.0;
	dampingRatio		= 1.0;
	for (unsigned row = X; row<=Z; row++) {
		for (unsigned col = X; col<=Z; col++) {
			if (row==col)
				envK[row][row] = 50000.0;
			else
				envK[row][col] = 0.0;
			envB[row][col] = 2*sqrt( 2*dampingRatio*mass*envK[row][col] );
		}
	}
	pForce				= NULL;
	pSphere				= NULL;
	pBlock				= NULL;

	curFor				= vector<double>(NAXES, 0.0);
	curGim				= vector<double>(NAXES, 0.0);

	/// <summary>
	/// data collection settings
	/// </summary>
	duration			= 5.0; // s
	sRate				= 2500;	// Hz
	dataVec				= vector<double>(NSENSORST, 0.0);
	dataMat				= vector<vector<double>>((unsigned) ceil(NSENSORST*duration*sRate), vector<double>(NSENSORST, 0.0));
	dataBuf				= NULL;
	fileName			= L".\\data.mat";

	curFor				= vector<double>(NAXES, 0.0);
	curPos				= vector<double>(NAXES, 0.0);
	curGim				= vector<double>(NAXES, 0.0);

	zeroPos				= vector<double>(NAXES, 0.0);
	zeroGim				= vector<double>(NAXES, 0.0);
}

ChmDevice::~ChmDevice(void)
{
	Disconnect();
	delete hm;
	delete dataBuf;
}

bool ChmDevice::Connect(wstring str = L"vmd")
{
	// try connecting to the hapticMASTER
	deviceName	= str;
	hm			= ConnectToHapticMASTER((char *) deviceName.c_str());
	Sleep(1000);
	if (hm == NULL) // if the hapticMASTER is not connected
		return false;

	// try initializing the hapticMASTER
	GetState();
	if ( (currentState != FCSSTATE_NORMAL) && (currentState != FCSSTATE_FIXED) && (currentState != FCSSTATE_FREE) ) {
			 hm->SetRequestedState(FCSSTATE_INITIALISING);
			 while ( currentState != FCSSTATE_INITIALISED ) {
				 Sleep(1000);
				 GetState();
			 }
	}
	// add connection checking here

	hm->DeleteAll(); // delete all haptic objects
	SetMass(mass);   // set mass

	/// Remove all existing info blocks and add only the ones below.
	/// This sets up the order in which the data is collected.
	hm->RemoveInfoBlock(FCSPRM_ALL);		// remove all data collection
	hm->AddInfoBlock(FCSPRM_POSITION);		// add postion
	hm->AddInfoBlock(FCSPRM_VELOCITY);		// add velocity
	hm->AddInfoBlock(FCSPRM_ACCELERATION);	// add acceleration
	hm->AddInfoBlock(FCSPRM_FORCE);			// add force
	hm->AddInfoBlock(FCSPRM_POTMETERS);		// add gimbal
	
	return true;
}

bool ChmDevice::Disconnect() {
	if (hm == NULL)
		return true;
			
	hm->GetCurrentState(currentState);
	if ( (currentState != FCSSTATE_OFF) && (currentState != FCSSTATE_FAIL) ) {
		hm->SetRequestedState(FCSSTATE_OFF);

		while (currentState != FCSSTATE_OFF) {
			hm->GetCurrentState(currentState);
			Sleep(100);
		}
	}
	hm->DeleteAll(); // delete all haptic objects

	/// <summary>
	/// Remove all existing info blocks.
	/// </summary>
	hm->RemoveInfoBlock(FCSPRM_ALL);
	
	return true;
}

bool ChmDevice::SetState(FCSSTATE state) {
	unsigned timeOut = 0;

	// check if hapticMASTER exists
	if (hm==NULL)
		return false;

	// check if current state and the requested state are the same
	GetState();
	if (currentState == state)
		return true;
	
	// if the requested state is not FCSSTATE_INITIALISED,
	// set it it FCSSTATE_INITIALISED first
	if (state != FCSSTATE_INITIALISED) {
		if ( !SetState(FCSSTATE_INITIALISED) )
			return false;
	}

	// request to set the state
	if ( hm->SetRequestedState(state) ) {
		// if setting the state failed
		return false;
	}
	else {
		// if the state was successfully requested
		// ensure that the state was set
		while (currentState!=state && timeOut<10) {
			GetState();
			Sleep(100);
			timeOut++;
		}
	}

	return (currentState == state) ? true : false;
}

FCSSTATE ChmDevice::GetState(void) {
	// check if hapticMASTER exists
	if (hm==NULL)
		currentState = FCSSTATE_UNKNOWN;
	else
		hm->GetCurrentState(currentState);

	return currentState;
}

bool ChmDevice::SetMass (double mass) {
	this->mass = mass;
	return (!hm->SetParameter(FCSPRM_INERTIA, mass)); 
}

void ChmDevice::SetEnvironment(double stiffness[3][3], double damping[3][3]) {
	for (unsigned row=X; row<=Z; row++) {
		for (unsigned col=X; col<=Z; col++) {
			envK[row][col] = stiffness[row][col];
			envB[row][col] = damping[row][col];
		}
	}
}

void ChmDevice::SetEnvironment(double stiffness[3][3], double zeta) {
	dampingRatio = zeta;
	for (unsigned row=X; row<=Z; row++) {
		for (unsigned col=X; col<=Z; col++) {
			envK[row][col] = stiffness[row][col];
			envB[row][col] = 2*sqrt( 2*dampingRatio*mass*envK[row][col] );
		}
	}
}

void ChmDevice::CreateSphericalLimits(double radius, double center[3]) {
	pSphere = hm->CreateSphere(center, radius, 50000.0, 500.0, 1.0, 1.2, 0.5, 0.05);
}

void ChmDevice::CreateCuboidalLimits(double lengths[3], double center[3]) {
	double orientation[] = {0.0, 0.0, 0.0};
	pBlock = hm->CreateBlock(center, orientation, lengths, 50000, 500, 1.0, 1.5, 0.5, 0.05);
}
void ChmDevice::CreateConstantForce(double force[3]) {
	pForce = hm->CreateConstantForce(force);
}


bool ChmDevice::ArmCollection(void) {
	/// <summary>
	/// needs implementation of the following steps:
	/// 1. verify hm exists
	/// 2. set the filename, duration and sRate
	/// 3. open the matlab file
	/// </summary>

	return false;
}

bool ChmDevice::CollectData(void) {
	/// <summary>
	/// 0. Verify hm exists
	/// 1. start the scope to collect
	/// 2. end scope, confirm collected
	/// </summary>

	return false;
}
/*
wstring ChmDevice::SaveData(wstring fName) {

	//using namespace matlab;
	// open mat file
	char* matfName = new char[255];
	sprintf(matfName,"%ls", fName.c_str());
		
	matlab::MATFile *pmat;
	matlab::mxArray *sensorData;
	double *tempPtr;
	int status;
	
	pmat = matlab::matOpen(matfName, "w");
	if (pmat == NULL) {
		errStr = L"File was not created.";
		return errStr;
	}

	// create matlab array to and allocate memory
	sensorData = matlab::mxCreateDoubleMatrix( dataMat.size() / NSENSORST, NSENSORST, matlab::mxREAL );
	if (sensorData == NULL) {
		errStr = L"Could not create matlab matrix.";
		return errStr;
	}

	// copy data to matlab array -- memcpy doesn't work
	tempPtr = matlab::mxGetPr(sensorData);
	for (unsigned i = 0; i<data.size()/NSENSORST; i++) // time stamp
		for (unsigned j = 0; j<NSENSORST; j++) // sensor
			tempPtr[j*dataMat.size()/NSENSORST + i] = dataMat[i*NSENSORST + j]; // note transpose when going from C++ to MATLAB
	
	// store it into the file as variable "hmdata"
	status = matlab::matPutVariable(pmat, "hmdata", sensorData);
	if (status != 0) {
		errStr = L"Could not copy variable into matlab array.";
		return errStr;
	} 

	// free up the memory space (doesn't remove the variable itself
	matlab::mxDestroyArray(sensorData);
		
	if (matlab::matClose(pmat) != 0) {
		errStr = L"Error in closing matlab file.";
		return errStr; 
	}
	errStr = L"Success.";
	return errStr;
}
*/